#ifndef CALIB
#define CALIB
 
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/shm.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <vector>
#include <string>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <tf/transform_broadcaster.h>
using namespace std;
using namespace cv;
using PointType = pcl::PointXYZI;
 
class Calib{
    public:
        Eigen::Matrix4f R0_rect = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f P2 = Eigen::Matrix4f::Identity();
        float p2[12] = {604.410, 0, 319.579, 0, 0, 604.523, 231.629, 0, 0, 0, 1, 0};
        Eigen::Matrix4f Trv2c = Eigen::Matrix4f::Identity();
        float trv2c[12] = {0, -1, 0, 0, 
                                            0, 0, -1, -0.133, 
                                            1, 0, 0, -0.23};
        pcl::PointCloud<PointType>::Ptr p_pc_frstum;
        int img_h = 480;
        int img_w = 640;
        cv::Mat depth_img;

    public:
        Calib(){ 
            for (int i = 0; i<12; i++){
                P2(i/4, i%4) = p2[i];
                Trv2c(i/4, i%4) = trv2c[i];
            }
            // memcpy(P2.data(), p2, sizeof(p2));
            // memcpy(Trv2c.data(), trv2c, sizeof(trv2c));
            // cout<<"P2 "<<P2.block(0, 0, 4, 4)<<endl;
            // cout<<"Trv2c "<<Trv2c.block(0, 0, 4, 4)<<endl;
         }
          
        ~Calib() {
        }   

        void get_frstum_point_depth_img(pcl::PointCloud<PointType>::Ptr p_pc){
            p_pc_frstum.reset(new pcl::PointCloud<PointType>);
            depth_img = Mat(img_w, img_h, CV_32FC1,Scalar(0) );
            Eigen::Vector3f pt;
            for (auto& p : p_pc->points){
                pt = (P2*Trv2c*Eigen::Vector4f(p.x, p.y, p.z, 1)).topRows(3);
                int32_t u = static_cast<int32_t>(pt(0)/pt(2));
                int32_t v = static_cast<int32_t>(pt(1)/pt(2));
                if (u>=0 && u<= img_w && v >= 0 && v <= img_h ){
                    p_pc_frstum->points.push_back(p);
                    depth_img.at<float>(v, u ) = pt(2) - P2(2,3);
                    // cout<< "depth"<< depth_img.at<float>(v, u )<<" ";
                }
            }
            // cout<<endl;
        }

        Eigen::Vector4f img2lidar(int u, int v, float depth){
            Eigen::Vector4f pt;
            pt(0) = ((u - P2(0, 2))*depth ) / P2(0,0) + P2(0,3) / (-P2(0,0));
            pt(1) = ((v - P2(1, 2))*depth ) / P2(1,1) + P2(1,3) / (-P2(1,1));
            pt(2) = depth;
            pt(3) = 1;
            // pt = (Trv2c.inverse()*pt).topRows(3);
            pt = (Trv2c.inverse()*pt);
            return pt;
        }

        Eigen::Vector4f img2camera(int u, int v, float depth){
            Eigen::Vector4f pt;
            pt(0) = ((u - P2(0, 2))*depth ) / P2(0,0) + P2(0,3) / (-P2(0,0));
            pt(1) = ((v - P2(1, 2))*depth ) / P2(1,1) + P2(1,3) / (-P2(1,1));
            pt(2) = depth;
            pt(3) = 1;
            return pt;
        }
};

// class PcDataLoader{
//     public:
        
// }
#endif
